#ifndef WALKAWARE_SHARED_PLANEDETECTOR_H
#define WALKAWARE_SHARED_PLANEDETECTOR_H

#include <vector>
#include <limits>

#include <vcg/space/fitting3.h>

#include <WalkaWare/Shared/Plane3.h>
#include <WalkaWare/Shared/Box3.h>
#include <WalkaWare/Shared/PointCloud.h>

namespace WalkaWare
{

namespace Shared
{

class PlaneDetector
{
	public:

		typedef void          BaseType;
		typedef PlaneDetector ThisType;

		bool detect(const PointCloud & pointCloud, const Box3f & bbox, const Vec3f & preferredNormal, Plane3f & plane, float & error)
		{
			if (pointCloud.isEmpty()) return false;
			if (bbox.IsEmpty() || bbox.IsNull()) return false;

			// select points inside bounding box
			std::vector<Vec3f> points;
			const int n = pointCloud.size();
			for (int i=0; i<n; ++i)
			{
				if (bbox.IsIn(pointCloud[i]))
				{
					points.push_back(pointCloud[i]);
				}
			}
			if (points.empty()) return false;

			const int m = points.size();

			// RANSASC
			const int          maxIterations    = 20;
			const int          minConsensusSize = int(float(m) * 0.10f);
			const float        errorThreshold   = bbox.Diag() * 0.05f;

			const float        maxDist2 = errorThreshold * errorThreshold;

			Plane3f            bestPlane(0.0f, Vec3f(0.0f, 1.0f, 0.0f));
			float              bestError = std::numeric_limits<float>::max();
			int                bestConsensusSetSize = 0;

			int                indices[3] = { 0, 0, 0 };
			std::vector<Vec3f> testConsensusSet;

			for (int i=0; i<maxIterations; ++i)
			{
				ThisType::random3(m, indices);

				Plane3f testPlane;
				testPlane.Init(points[indices[0]], points[indices[1]], points[indices[2]]);

				const float cosAngle = float(fabs(preferredNormal * testPlane.Direction()));

				testConsensusSet.resize(0);
				testConsensusSet.push_back(points[indices[0]]);
				testConsensusSet.push_back(points[indices[1]]);
				testConsensusSet.push_back(points[indices[2]]);

				for (int j=0; j<m; ++j)
				{
					if ((j == indices[0]) || (j == indices[1]) || (j == indices[2])) continue;
					const float d = vcg::SquaredDistance(testPlane, points[j]);
					if (d > maxDist2) continue;
					testConsensusSet.push_back(points[j]);
				}

				const int p = int(testConsensusSet.size());
				if (p < minConsensusSize) continue;

				const Vec3f eigenValues = vcg::PlaneFittingPoints(testConsensusSet, testPlane);
				const float testError   = eigenValues[0] * (1.0f - cosAngle);
				if ((testError < bestError) || ((testError == bestError) && (bestConsensusSetSize < p)))
				{
					bestPlane = testPlane;
					bestError = testError;
					bestConsensusSetSize = p;
				}
			}

			if (bestConsensusSetSize <= 0) return false;

			plane = bestPlane;
			error = bestError;

			return true;
		}

		/*
		bool detect(const PointCloud & pointCloud, const Box3f & bbox, Plane3f & plane)
		{
			if (pointCloud.isEmpty()) return false;
			if (bbox.IsEmpty() || bbox.IsNull()) return false;

			// select points inside bounding box
			QVector<Vec3f> points;
			const int n = pointCloud.size();
			for (int i=0; i<n; ++i)
			{
				if (bbox.IsIn(pointCloud[i]))
				{
					points.push_back(pointCloud[i]);
				}
			}
			if (points.empty()) return false;

			const int m = points.size();

			// RANSASC
			const int          maxIterations    = 10;
			const int          minConsensusSize = int(float(m) * 0.5f);
			const float        errorThreshold   = bbox.Diag() * 0.1f;

			const float        maxDist2 = errorThreshold * errorThreshold;

			Plane3f            bestPlane(0.0f, Vec3f(0.0f, 1.0f, 0.0f));
			float              bestError = std::numeric_limits<float>::max();
			std::vector<int>   bestConsensusSet;

			Plane3f            testPlane(0.0f, Vec3f(0.0f, 1.0f, 0.0f));
			float              testError = std::numeric_limits<float>::max();
			std::vector<int>   testConsensusSet;

			std::vector<Vec3f> testConsensusPoints;
			int indices[3] = { 0, 0, 0 };

			for (int i=0; i<maxIterations; ++i)
			{
				ThisType::random3(m, indices);
				testPlane.Init(points[indices[0]], points[indices[1]], points[indices[2]]);

				testConsensusSet.resize(0);
				testConsensusSet.push_back(indices[0]);
				testConsensusSet.push_back(indices[1]);
				testConsensusSet.push_back(indices[2]);

				for (int j=0; j<m; ++j)
				{
					if ((j == indices[0]) || (j == indices[1]) || (j == indices[2])) continue;
					if (vcg::SquaredDistance(testPlane, pointCloud[j]) > maxDist2)   continue;
					testConsensusSet.push_back(j);
				}

				if (testConsensusSet.size() < minConsensusSize) continue;

				// copy points
				const int p = testConsensusSet.size();
				testConsensusPoints.resize(0);
				testConsensusPoints.resize(size_t(p));
				for (int j=0; j<p; ++j)
				{
					testConsensusPoints[j] = pointCloud[testConsensusSet[j]];
				}

				const Vec3f eigenValues = vcg::PlaneFittingPoints(testConsensusPoints, testPlane);
				testError = eigenValues[0];
				if ((testError < bestError) || ((testError == bestError) && (bestConsensusSet.size() < p)))
				{
					bestPlane = testPlane;
					bestError = testError;
					bestConsensusSet.swap(testConsensusSet);
				}
			}

			return (!bestConsensusSet.empty());
		}
		*/

	protected:

		static int random(int max)
		{
			//return (((qrand() * (RAND_MAX + 1)) + qrand()) % max);
			return int((float(qrand()) / float(RAND_MAX)) * float(max - 1));
		}

		static void random3(int max, int indices[3])
		{
			do { indices[0] = ThisType::random(max); } while (false);
			do { indices[1] = ThisType::random(max); } while (indices[0] == indices[1]);
			do { indices[2] = ThisType::random(max); } while ((indices[0] == indices[2]) || (indices[1] == indices[2]));
		}
};

} // end namespace Shared

} // end namespace WalkaWare

#endif // WALKAWARE_SHARED_PLANEDETECTOR_H
